/*
 * TRACKING.c
 *
 * Created: 14.11.2012 17:05:31
 *  Author: bardbakk
 */ 

#define DELAY 0.5   //sec
#define F_CPU 4915200

#include <avr/interrupt.h>
#include "definitions.h"
#include "USART.h"

static int first = TRUE;
static int last_x;
static int last_y;

void TRACKING_predict(int8_t* x, int8_t* y, float dt)
{
	if(first)
	{
		first = FALSE;
		last_x = *x;
		last_y = *y;
		return;
	}
	int x_speed = (*x - last_x)/dt;
	int y_speed = (*y - last_y)/dt;
	
	last_x = *x;
	last_y = *y;
	
	*x += x_speed*DELAY;
	*y += y_speed*DELAY;
	
		
}
